import serial
import matplotlib.pyplot as plt
import numpy as np
import time

# --- Serial setup ---
ser = serial.Serial('COM4', 115200, timeout=1)  # <-- Change COM port if needed
time.sleep(2)  # wait for ESP32 to reset

# --- Matplotlib setup ---
plt.ion()  # interactive mode
fig, ax = plt.subplots(subplot_kw={'projection': 'polar'})
ax.set_theta_zero_location("N")
ax.set_theta_direction(-1)
ax.set_ylim(0, 100)  # max distance in cm

# storage
angles_front, distances_front = [], []
angles_back, distances_back = [], []

while True:
    try:
        line = ser.readline().decode().strip()
        if not line:
            plt.pause(0.01)
            continue

        parts = line.split(',')
        if len(parts) != 3:
            continue

        angle_str, distance_str, sensor = parts
        try:
            angle = int(angle_str)
            distance = float(distance_str) if "OutOfRange" not in distance_str else None
        except:
            continue

        if distance is None or distance > 100:
            continue  # skip invalid/out of range

        # Convert to polar coords
        theta = np.deg2rad(angle)

        # Separate by sensor
        if sensor == "front":
            angles_front.append(theta)
            distances_front.append(distance)
            if len(angles_front) > 200:
                angles_front, distances_front = angles_front[-200:], distances_front[-200:]
        elif sensor == "back":
            angles_back.append(theta)
            distances_back.append(distance)
            if len(angles_back) > 200:
                angles_back, distances_back = angles_back[-200:], distances_back[-200:]

        # --- Clear and redraw ---
        ax.clear()
        ax.set_theta_zero_location("N")
        ax.set_theta_direction(-1)
        ax.set_ylim(0, 100)

        # Radar grid labels
        for r in [25, 50, 75, 100]:
            ax.text(np.deg2rad(5), r, f"{r}cm", color="white", fontsize=8)

        # Plot points
        ax.scatter(angles_front, distances_front, c='lime', s=25, label="Front Sensor")
        ax.scatter(angles_back, distances_back, c='cyan', s=25, label="Back Sensor")

        ax.legend(loc="lower left")

        plt.pause(0.01)  # keeps window responsive

    except KeyboardInterrupt:
        print("Exiting...")
        break

ser.close()